#include "Interial.h"
#include "imu.h"
#include "globalvariable.h"
#include "altitude.h"

#define BoardTimeConst   2.5f
#define DEADBAND        0.450f

INTERIAL Interial;
float k1_z=0.0f,k2_z=0.0f,k3_z=0.0f;
void Interial_Init(void)
{
	k1_z=3.0f/BoardTimeConst;
	k2_z=3.0f/BoardTimeConst/BoardTimeConst;
	k3_z=1.0f/BoardTimeConst/BoardTimeConst/BoardTimeConst;	
}

void Interial_Update(float dt)
{
	 Interial.acc_nog = Accwithoutg_Get();
	if(Interial.acc_nog<=10 && Interial.acc_nog>=-10)
	{
		Interial.acc_nog=0;
	}
	 Interial.velocity += Interial.acc_nog*dt;//cm/s
	 Interial.position += Interial.velocity*dt;//cm

	
//	static unsigned char runcnt=0;
//	float error=0.0f;
//	runcnt++;
//	if(runcnt>=250)	
//	{
//		runcnt=251;
//	 Interial.acc_nog=Accwithoutg_Get();
//	 error=Interial.acc_nog+Interial.acc_correct-Interial.first_accnog;
//		
//	 if(error>DEADBAND)
//	 {
//		 error-=DEADBAND;
//	 }
//	 else if(error>=-DEADBAND && error<=DEADBAND)
//	 {
//		 error=0;
//	 }
//	 else
//	 {
//	   error+=DEADBAND;
//	 }
//	   Interial.velocity =error*dt;//cm/s
//	   Interial.position+=Interial.velocity*dt;//cm
//	}
//	else
//	{
//		if(runcnt>=240)
//		{
//				Interial.first_accnog=Accwithoutg_Get();
//			return;
//		}
//		return;
//  }
	
	
//	Interial.acc_nog = Accwithoutg_Get();
//	Interial.correct+= Interial.position_error*k3_z*dt;
//	Interial.velocity+=Interial.position_error*k2_z*dt;
//	Interial.position_correct+=Interial.position_error*k1_z*dt;
//	Interial.velocity_increase=(Interial.acc_nog+Interial.acc_correct)*dt;
//	Interial.position_base+=(Interial.velocity+Interial.velocity_increase*0.5f)*dt;
//	Interial.velocity+=Interial.velocity_increase;
//	Interial.position=(Interial.position_base+Interial.position_correct)*0.2f+ALTitude*0.8f;

}	
